#include "hitechnic-sensormux.h"
#include "hitechnic-irseeker-v2.h"
#include "lego-touch.h"

tMUXSensor BtnLI = msensor_S4_1;
tMUXSensor IR_L = msensor_S4_2;	//Relative to the IR direction
tMUXSensor IR_R = msensor_S4_3;  //Relative to the IR direction
tMUXSensor BtnRA = msensor_S4_4;

#define W_Diamter 4
#define T_Rad 9.125
#define M_Standard 80
#define W_G_Ratio 2
#define POTS_break 200	//Number of time POTS must be vilated
#define POTS_target 0.85//Percent of power that must be reached before POTS can be considered vilated

//Distance from Ramp Edge to First Goal = ~7 feet
//Current Macanum Wheels have a diameter of 4 in.
//Distance from Parking to Center = ~4 feet to center from area edge
//Each tile has a length of 2 feet


const	int IR_AR[9][9] = { 	//9X9 array for sector value. 0 = DNE, negative = behind, 1, 73, 64, 10 is centered, others labled left to right
		{-1, 0, 0, 0, 0, 0, 0, 0, 91},		/*1:1, 2:1, 3:1, 4:1, 5:1, 6:1, 7:1, 8:1, 9:1*/
		{-5, 2, 3, 0, 0, 0, 12, 0, 13}, /*1:2, 2:2, 3:2, 4:2, 5:2, 6:2, 7:2, 8:2, 9:2*/
		{0, 0, 5, 6, 9, 14, 73, 0, 0}, /*1:3, 2:3, 3:3, 4:3, 5:3, 6:3, 7:3, 8:3, 9:3*/
		{0, 0, 0, 0, 8, 64, 15, 0, 0}, /*1:4, 2:4, 3:4, 4:4, 5:4, 6:4, 7:4, 8:4, 9:4*/
		{0, 0, 0, 0, 10, 21, 20, 18, 0},/*1:5, 2:5, 3:5, 4:5, 5:5, 6:5, 7:5, 8:5, 9:5*/
		{16, 16, 0, 0, 0, 0, 23, 0, 0 },/*1:6, 2:6, 3:6, 4:6, 5:6, 6:6, 7:6, 8:6, 9:6*/
		{0, 0, 0, 0, 0, 0, 24, 25, 0} , /*1:7, 2:7, 3:7, 4:7, 5:7, 6:7, 7:7, 8:7 9:7*/
		{0, 0, 0, 0, 0, 0, 0, 26, 0}  , /*1:8, 2:8, 3:8, 4:8, 5:8, 6:8, 7:8, 8:8, 9:8*/
		{0, 0, 0, 0, 0, 0, 0, -4, -2}};  /*1:9, 2:9, 3:9, 4:9, 5:9, 6:9, 7:9, 8:9, 9:9*/

const int R_AR[] = {15, 10, 6, 3, 2, -3, -6, -10, -15};
const int L_AR[]= {15, 10, 6, 3, 2, -3, -6, -10, -15};

void resetEncoders()
{ //sets encoders to 0 and clears second timer for POTS
	nMotorEncoder[B_Right] = 0;
	nMotorEncoder[B_Left] = 0;
	nMotorEncoder[F_Right] = 0;
	nMotorEncoder[F_Left] = 0;
	ClearTimer(T2);
}

int errors()	//Checks to confirm all sensors and servos are plugged in
{
	eraseDisplay();
	int L_dir, R_dir, raiseST, liftST, line = 1, nErrors = 0;
	L_dir = HTIRS2readACDir(IR_L);
	R_dir = HTIRS2readACDir(IR_R);
	raiseST = 1;
	liftST = 1;
	raiseST = TSreadState(BtnRA);
	liftST = TSreadState(BtnRA);
	nVolume = 4;
	if(L_dir == -1)
	{
	    nxtDisplayTextLine(line, "IR_L (Mult 2)");
	    PlaySound(soundBeepBeep);
	    line++;
	    nErrors++;
	    wait10Msec(50);
	}
	if(R_dir == -1)
	{
	    nxtDisplayTextLine(line, "IR_R (Mult 3)");
	    PlaySound(soundBeepBeep);
	    line++;
	    nErrors++;
	    wait10Msec(50);
	}
	if(servoTarget[Hook] < 47 || servoTarget[Hook] > 53)
	{
		nxtDisplayTextLine(line, "Hook (servo1)");
	  PlaySound(soundBeepBeep);
	  line++;
	  nErrors++;
	  wait10Msec(50);
	}
	if(servoTarget[Scooper] < 172 || servoTarget[Scooper] > 178)
	{
		nxtDisplayTextLine(line, "Scooper (servo6) Error");
	  PlaySound(soundBeepBeep);
	  line++;
	  nErrors++;
	  wait10Msec(50);
	}
	if(servoTarget[Hook] < 1 || servoTarget[Hook] > 3)
	{
		nxtDisplayTextLine(line, "Guide(servo2)");
	  PlaySound(soundBeepBeep);
	  line++;
	  nErrors++;
	  wait10Msec(50);
	}
	if(raiseST == 1)
	{
		nxtDisplayTextLine(line, "Raise touch (Mult 1)");
	  PlaySound(soundBeepBeep);
	  line++;
	  nErrors++;
	  wait10Msec(50);
	}
	if(liftST == 1)
	{
		nxtDisplayTextLine(line, "Lift(Mult 3) ");
	  PlaySound(soundBeepBeep);
	  line++;
	  nErrors++;
	  wait10Msec(50);
	}
	if(nErrors > 0)
	{
		wait10Msec(600);
		eraseDisplay();
	}
	nVolume = 0;
	return nErrors;
}


int raise_lift()	//Raises the lift to the match posistion
{
	int raiseST;
	while((raiseST == TSreadState(BtnRA)) == 0)
	{
		motor[Raise] = -15;
		writeDebugStreamLine("%d, %d", raiseST, motor[Raise]);
		//motor[Lift] = 30;
	}
	motor[Raise] = 0;
	motor[Lift] = 0;
	return raiseST;
}

void extend_lift()	//Extends the lift to maximum height
{
	int liftST = TSreadState(BtnLI);
	while(liftST == 0)
	{
		liftST = TSreadState(BtnLI);
		motor[Lift] = -30;
	}
	motor[Lift] = 0;
}


void extend_liftT(int time)	//Extends the lift for a certain amount of time in seconds
{
	int liftST = TSreadState(BtnLI);
	//bool extending;
	//if(extending == false)
	ClearTimer(T1);
	while(time100[T1] < (time * 10))
	{
		if(liftST == 0)
		{
			//extending = true;
			liftST = TSreadState(BtnLI);
			motor[Lift] = -30;
		}
	}
	motor[Lift] = 0;
}


void lower_liftT(int time)	//Lowers the lift for a certain amount of time in seconds
{
	int liftST = TSreadState(BtnLI);
	ClearTimer(T1);
	while(time100[T1] < (time * 10) || liftST == 0)
	{
		liftST = TSreadState(BtnLI);
		motor[Lift] = 20;
	}
	motor[Lift] = 0;
}


void retract_lift()	//Retracts the lift back into prematch condition. Used with lower_liftT for packing
{
	int raiseST = TSreadState(BtnRA);
	ClearTimer(T1);
	while(time100[T1] < 50)
	{
	  raiseST = TSreadState(BtnRA);
		motor[Raise] = 15;
		motor[Lift] = -15;
	}
	motor[Raise] = 0;
	motor[Lift] = 0;
}

int getIRSector()	//Calculates the more precise direction of the IR beacon with 2 instead of 1 sensor
{
	int sector = 0;
	int L_dir = HTIRS2readACDir(IR_L) - 1;
	int R_dir = HTIRS2readACDir(IR_R) - 1;
	if(L_dir > -1 && R_dir > -1 && R_dir < 8 && L_dir < 8)
		 sector = IR_AR[R_dir][L_dir];
	writeDebugStreamLine("L_dir = %d  R_dir = %d  Sector = %d", L_dir, R_dir, sector);
	return sector;
}

int motorSmootherL(int before, int after)	//Reduces the change in the Launcher speed for motor preservation
{
		int nSmooth = 5;
		if(before < after)
		{
		 after = before + nSmooth;
		}
	if(before > after){
		after = before - nSmooth;
	}
	else if(before == after)
	{
		return after;
	}

	return after;
 }

void dataLogger()	//Takes a snapshot of the robots condition every second to be reviewed later
{
	int counter = 30, sector, L_dir, R_dir, eBL, eFL, eBR, eFR, raiseST, liftST, sTime_prev, sTime;
	float circum, timer, speedBL, speedFL, speedBR, speedFR, distFL, distFR, distBL, distBR, pFL, pFR, pBL, pBR;
	circum = (PI * W_Diamter) / W_G_Ratio;

	if(counter == 30)
  	writeDebugStreamLine("MATCH START---------");

	sTime = nSysTime;

	if((sTime_prev - sTime) < -1000 || counter == 30)
	{
		writeDebugStreamLine("%d: ", counter);
		wait10Msec(100);
		raiseST = TSreadState(BtnRA);
		liftST = TSreadState(BtnLI);
		eBL = abs(nMotorEncoder[B_Left]);
	  eFL = abs(nMotorEncoder[F_Left]);
	  eBR = abs(nMotorEncoder[B_Right]);
	  eFR = abs(nMotorEncoder[F_Right]);

	  timer = time100[T2] / 10;
  	speedBL = eBL / timer;
	  speedFL = eFL / timer;
	  speedBR = eBR / timer;
	  speedFR = eFR / timer;

	  pFL = (speedFL / 2958) * 100;
	  pFR = (speedFR / 2796) * 100;
	  pBL = (speedBL / 2986) * 100;
	  pBR = (speedBR / 2737) * 100;

		L_dir = HTIRS2readACDir(IR_L);
		R_dir = HTIRS2readACDir(IR_R);
		sector = getIRSector();
		distFL = (abs(nMotorEncoder[F_Left]) / 1120) * circum;
		distFR = (abs(nMotorEncoder[F_Right]) / 1120) * circum;
		distBL = (abs(nMotorEncoder[B_Left]) / 1120) * circum;
		distBR = (abs(nMotorEncoder[B_Right]) / 1120) * circum;
		writeDebugStreamLine("SENSORS  L_dir = %d  R_dir = %d  Sector = %d Raise = %s Lift = %s", L_dir, R_dir, sector, raiseST ? "Up" : "Down", liftST ? "Up" : "Down");
		writeDebugStreamLine("MOTORS   FL = %d  FR = %d  BL = %d  BR = %d  PT = %d ", motor[F_Left], motor[F_Right], motor[B_Left], motor[B_Right], motor[Launcher]);
		writeDebugStreamLine("ENCODERS  FL = %d  FR = %d  BL = %d  BR = %d", nMotorEncoder[F_Left], nMotorEncoder[F_Right], nMotorEncoder[B_Left], nMotorEncoder[B_Right]);
		writeDebugStreamLine("DISTANCE FL = %f  FR = %f  BL = %f  BR = %f", distFL, distFR, distBL, distBR);
		writeDebugStreamLine("POTS     FL = %f  FR = %f BL = %f  BR = %f", pFL, pFR, pBL , pBR);
		counter--;
		sTime_prev = sTime;

	}
	if(counter == 0)
		writeDebugStreamLine("MATCH END---------");
	return;

}


int pots(int times)	//Checks to see if motors are spinning the desired speed then returns the number of times in a row they've failed
{
	int motBR, motFR, motFL, motBL, eBL, eFL, eBR, eFR;
	float speedBL, speedFL, speedBR, speedFR, break_target, targetBR, targetBL, targetFL, targetFR, timer;


	motBR = motor[B_Right];
	motBL = motor[B_Left];
	motFL = motor[F_Left];
	motFR = motor[F_Right];

	eBL = abs(nMotorEncoder[B_Left]);
	eFL = abs(nMotorEncoder[F_Left]);
	eBR = abs(nMotorEncoder[B_Right]);
	eFR = abs(nMotorEncoder[F_Right]);

	timer = time100[T2] / 10;
	speedBL = eBL / timer;
	speedFL = eFL / timer;
	speedBR = eBR / timer;
	speedFR = eFR / timer;

	targetBL = 2986 * POTS_target;
	targetFL = 2958 * POTS_target;
	targetBR = 2737 * POTS_target;
	targetFR = 2796 * POTS_target;
	//writeDebugStreamLine("SPEEDS FL = %d  FR = %d  BL = %d  BR = %d", speedFL, speedFR, speedBL, speedBR);
	if(speedBL < targetBL || speedFL < targetFL || speedBR < targetBR || speedFR < targetBR)
		times++;
	else if(speedBL > targetBL && speedFL > targetFL && speedBR > targetBR && speedFR > targetBR)
		times = 0;

		return times;
}




int seekIR(int break_sector, int lExt)	//Uses the IR sensors to control the respective motors to align with the center goal with the option to extend the
{																				//Lift to full height.
	writeDebugStreamLine("seekIR(%d, %d)", break_sector, lExt);
	//float height;
	//float height_thresh = 1.5;
	int L_dir = HTIRS2readACDir(IR_L) - 1;
	int R_dir = HTIRS2readACDir(IR_R) - 1;
	int liftST, times;
	if(L_dir > -1&& R_dir > -1)
		int sector = IR_AR[R_dir][L_dir];
	int RMV, LMV, mult = 15;

	int sect_prev = sector;
	while(sector != break_sector && sector != 1)
	{
		//height = nMotorEncoder[Lift]
		//liftST = TSreadState(BtnLI);
		sect_prev = sector;
		sector = getIRSector();
		L_dir = HTIRS2readACDir(IR_L) - 1;
		R_dir = HTIRS2readACDir(IR_R) - 1;
		if(R_dir > -1)
			RMV = R_AR[R_dir] * mult;
		if(L_dir > -1)
			LMV = L_AR[L_dir] * mult;

		if(L_dir == 0)
			LMV = RMV;
		if(R_dir == 0)
			RMV = LMV;

		motor[F_Left] = LMV;
		motor[B_Left] = LMV;
		motor[F_Right] = RMV;
		motor[B_Right] = RMV;

		/*times = pots(times);
		if(times > POTS_break)
		{
			writeDebugStreamLine("POTS!!!!!!!!!");
			break;
		}*/
		/*if(liftST != lExt)
				motor[Lift] = -30;
			if(liftST == lExt)
				motor[Lift] = 0;*/

			/*if(height < (heightT + height_thresh) && height >  (heightT - height_thresh))
				motor[Lift] = 0;*/

		/*if(break_sector == 64 && sector == 73)
			break;*/
	}
	motor[B_Left] = 0;
	motor[F_Right] = 0;
	motor[B_Right] = 0;
	motor[F_Left] = 0;
	return sect_prev;
}





void rotate(char dir, int deg, int raised)	//Rotates the robot a specific direction for an approximate number of degrees
{
	writeDebugStreamLine("rotate(%c %d, %d)", dir, dir, deg);
	resetEncoders();
	int times, raiseST;
	float mult = ((1120 * 3) + 120) / 90;
	float turn = deg * mult; // something to do with how much the bot turns when "rotate" is used
	float enc_target = turn;
	bool raise_bool;
 /* while absolute value of the front left encoder is less than "enc_targe" and less than the
  absolute value of the front right encoder, as well as enc target and the absolute value of the
  back left encoder */
	while(abs(nMotorEncoder[F_Left]) < enc_target && abs(nMotorEncoder[F_Right]) < enc_target && abs(nMotorEncoder[B_Left]) < enc_target
		&& abs(nMotorEncoder[B_Right]) < enc_target)
	{
		raiseST = TSreadState(BtnRA);
			if(raiseST != raised && raise_bool == false)
				motor[Raise] = -7;
			if(raiseST == raised || time100[T1] > 30)
			{
				motor[Raise] = 0;
				raise_bool = true;
			}
		if(dir == 0 || dir == 'l')
		{
			motor[B_Right] = M_Standard;
			motor[B_Left] = M_Standard;
			motor[F_Right] = M_Standard;
			motor[F_Left] = M_Standard;
		}
		if(dir == 1 || dir == 'r')
		{
			motor[B_Right] = - M_Standard;
			motor[B_Left] = - M_Standard;
			motor[F_Right] = - M_Standard;
			motor[F_Left] = - M_Standard;
		}

		/*times = pots(times);
		if(times > POTS_break)
		{
			writeDebugStreamLine("POTS!!!!!!!!!");
			break;
		}*/
	}
	motor[B_Right] = 0;
	motor[B_Left] = 0;
	motor[F_Right] = 0;
	motor[F_Left] = 0;
	resetEncoders();
	wait10Msec(5);
}




void drive_speed(char dir, int dist, int speed)	//Drives the robot fowards or backwards a specific distance (inches) and speed (80% is most effecient)
{
	writeDebugStreamLine("drive_speed(%c %d, %d, %d)", dir, dir, dist, speed);
	int times = 0;
	float circum = W_Diamter * PI;
	float enc_target = ((dist / circum) * 1120) * W_G_Ratio;

	while(abs(nMotorEncoder[F_Right]) < enc_target && abs(nMotorEncoder[F_Left]) < enc_target && abs(nMotorEncoder[B_Left]) < enc_target && abs(nMotorEncoder[B_Right])
		< enc_target)
	{
		motor[Launcher] = 0;

		if(dir == 0 || dir == 'f')
		{
			motor[B_Right] = speed;
			motor[B_Left] = -speed;
			motor[F_Right] = speed;
			motor[F_Left] = -speed;
		}
		if(dir == 1 || dir == 'r')
		{
			motor[B_Right] = -speed;
			motor[B_Left] = speed;
			motor[F_Right] = -speed;
			motor[F_Left] = speed;
		}
		/*times = pots(times);
		if(times > POTS_break)
		{
			writeDebugStreamLine("POTS!!!!!!!!!");
			break;
		}*/
	}
	//writeDebugStreamLine("BL = %d, BR = %d, FR = %d, FL = %d", motor[B_Left], motor[B_Right], motor[F_Left], motor[F_Right]);
	motor[B_Right] = 0;
	motor[B_Left] = 0;
	motor[F_Right] = 0;
	motor[F_Left] = 0;
	resetEncoders();
	wait10Msec(5);

}



void drive_full(char dir, int dist, int speed, int raised)	//The same as drive_speed with the options of extending the lift to full height
{																																										//as well as controlling the scoop
	writeDebugStreamLine("drive_full(%c, %d, %d, %d, %d, %f, %d, %d, %d, %d)", dir, dist, speed, raised);
	float circum = W_Diamter * PI;
	float enc_target = ((dist / circum) * 1120) * W_G_Ratio;
	int liftST, times, raiseST, wall_dist, wall_range = 0, dist_change, wall_dist_prev, motors = 40, n = 0, distD1 = 0, distD2 = 0, m =0;
	bool use_pots = true, raise_bool = false;

	while(/*distD1 < dist && distD2 < dist*/abs(nMotorEncoder[F_Right]) < enc_target && abs(nMotorEncoder[F_Left]) < enc_target && abs(nMotorEncoder[B_Left]) < enc_target && abs(nMotorEncoder[B_Right])
		< enc_target)
	{


		//dataLogger();
		raiseST = TSreadState(BtnRA);
			if(raiseST != raised && raise_bool == false)
				motor[Raise] = -7;
			if(raiseST == raised || time100[T1] > 30)
			{
				motor[Raise] = 0;
				raise_bool = true;
			}



		if((dir == 0 || dir == 'f') )
		{
			motor[B_Right] = speed;
			motor[B_Left] = -speed;
			motor[F_Right] = speed;
			motor[F_Left] = -speed;
		}
		if((dir == 1 || dir == 'r'))
		{
			motor[B_Right] = -speed;
			motor[B_Left] = speed;
			motor[F_Right] = -speed;
			motor[F_Left] = speed;
		}
		distD1 += (abs(nMotorEncoder[F_Right]) / 1120) * circum;
		distD2 += (abs(nMotorEncoder[B_Left]) / 1120) * circum;
		/*times = pots(times);
		if(times > POTS_break && use_pots)
		{
			writeDebugStreamLine("POTS!!!!!!!!!");
			break;
		}*/
			raiseST = TSreadState(BtnRA);
			if(raiseST != raised && raise_bool == false)
				motor[Raise] = -7;
			if(raiseST == raised || time100[T1] > 30)
			{
				motor[Raise] = 0;
				raise_bool = true;
			}
			wall_dist_prev = wall_dist;
			wall_dist = SensorValue(Ultra);
			dist_change = wall_dist - wall_dist_prev;

		//writeDebugStreamLine("raiseST = %d, raised = %d Raise = %d", raiseST, raised, motor[Raise] );


			/*if(maintain > 0 && wall_dist < 255)
			{
				if(wall_dist > (maintain + maint_range) || wall_dist < (maintain - maint_range))
				{
					wall_range++;
					if(wall_range > 5)
					{
					while(dist_change < 0 || n == 0 || n == 1)
					{
						motor[B_Right] = -motors;
						motor[B_Left] = -motors;
						motor[F_Right] = -motors;
						motor[F_Left] = -motors;
						wall_dist_prev = wall_dist;
						wall_dist = SensorValue(Ultra);
						dist_change = wall_dist - wall_dist_prev;
						writeDebugStreamLine("%d  dist_change = %d", wall_dist, dist_change);
						if(dist_change > 0)
						{
							motors = -motors;

						}





				while(wall_dist > (maintain + maint_range))
				{
					motor[F_Left] = motors;
					motor[F_Right] = motors;
					motor[B_Left] = -motors;
					motor[B_Right] = -motors;
					wall_dist = SensorValue(Ultra);
					if(wall_dist < (maintain + maint_range))
						break;
				}

				while(wall_dist < (maintain - maint_range))
					{
						motor[F_Left] = -motors;
						motor[F_Right] = -motors;
						motor[B_Left] = motors;
						motor[B_Right] = motors;
						wall_dist = SensorValue(Ultra);
						if(wall_dist > (maintain - maint_range))
							break;
					}

					resetEncoders();
				}
					if(wall_dist < (maintain + maint_range) && wall_dist > (maintain - maint_range))
						{
							wall_range = 0;
							use_pots = true;
						}
					}

				}

				//liftST = TSreadState(BtnLI);
			/*if(liftST != lExt)
				motor[Lift] = -30;
			if(liftST == lExt)
				motor[Lift] = 0;*/

			/*if(height < (heightT + height_thresh) && height >  (heightT - height_thresh))
				motor[Lift] = 0;*/

			/*if(servoTarget[Scooper] < (scoopT + 1))
				servoTarget[Scooper] += dScoop;
			if(servoTarget[Scooper] > (scoopT + 1))
				servoTarget[Scooper] -= dScoop;
			else if(dScoop == NULL)
				servoTarget[Scooper] = scoopT;

	}*/
			raiseST = TSreadState(BtnRA);
			if(raiseST != raised && raise_bool == false)
				motor[Raise] = -7;
			if(raiseST == raised || time100[T1] > 30)
			{
				motor[Raise] = 0;
				raise_bool = true;
			}

	}
	motor[B_Right] = 0;
	motor[B_Left] = 0;
	motor[F_Right] = 0;
	motor[F_Left] = 0;
	resetEncoders();
	wait10Msec(5);

}



void drive(char dir, float dist)	//Drives the robot fowards or backwards a specific distance standard (now full) speed
{
	drive_speed(dir, dist, M_Standard);
	}

void drivef(char dir, int dist)
{
	drive_speed(dir, dist, 80);
}

void translate(char dir, float dist)	//Translates the robot left or right a specific distance (needs to be calibrated)
{
	int enc_target = (1120 * W_G_Ratio) / 3;
	while(abs(nMotorEncoder[F_Right]) < enc_target && abs(nMotorEncoder[B_Left]) < enc_target
		&& abs(nMotorEncoder[B_Right]) < enc_target && abs(nMotorEncoder[F_Left]) < enc_target)
	{
		if(dir == 0 || dir == 'l' )
		{
			motor[F_Left] = M_Standard;
			motor[F_Right] = M_Standard;
			motor[B_Left] = -M_Standard;
			motor[B_Right] = -M_Standard;
		}

		if(dir == 1 || dir == 'r')
		{
			motor[F_Left] = -M_Standard;
			motor[F_Right] = -M_Standard;
			motor[B_Left] = M_Standard;
			motor[B_Right] = M_Standard;
		}
	}

	motor[F_Left] = 0;
	motor[F_Right] = 0;
	motor[B_Right] = 0;
	motor[B_Left] = 0;
	resetEncoders();

}

void hooke(int ext)	//Extends or retracts the hook
{
	writeDebugStreamLine("hooke(%d", ext);
	/*values 245 & 50 were measured using another program; value 245 makes the
	servo grab the rolling goal, value 50 releases the rolling goal*/
	if(ext == 0)
	{
		servoTarget[Hook] = 50;
	}
	if(ext == 1)
	{
		servoTarget[Hook] = 245;
	}



}

void launch(int lupe)	//Launches a specified number of balls
{
	writeDebugStreamLine("Launch(%d)", lupe);
	motor[Launcher] = -70;
	wait10Msec(75);
	for (int i = 0; i < lupe; i++)
	{

	  servoTarget[Guide] = 232;
	  wait1Msec (250);
  	servoTarget[Guide] = 237;//<== Jeremy's function
	  wait1Msec(750);

  }
  while (motor[Launcher] > 10)
  {
  	int launch_prev = motor[Launcher];
  	motor[Launcher] = motorSmootherL(launch_prev, launch_prev);
  }
	motor[Launcher] = 0;

	return;
}

/*void scoop (int nTarget) //Moves the scoop to a specified target position slowly (as not to catupult balls)
{
	writeDebugStreamLine("scoop(%d)", nTarget);
	if (servoTarget[Scooper] < nTarget)
	{
		while (servoTarget[Scooper] < nTarget)
		{
			int delta = nTarget - servoTarget[Scooper];//<== Jeremy's Function
			if (delta > 10)
			{
				delta = 10;
				wait1Msec (100);
			}
			servoTarget[Scooper] += delta;
		}
	}
	else
	{
		if (servoTarget[Scooper] > nTarget)
	{
		while (servoTarget[Scooper] > nTarget)
		{
			int delta = nTarget + servoTarget[Scooper];
			if (delta < 10)
			{
				delta = 10;
				wait1Msec (100);
			}
			servoTarget[Scooper] -= delta;
		}
	}
	}
}*/
